#include "Projection_GPU.h"
#include "ApplyRigidTransform.h"


__global__ void initTemp(float3* temp, int width, int height){
	int x = blockIdx.x * blockDim.x + threadIdx.x;
	int y = blockIdx.y * blockDim.y + threadIdx.y;

	temp[x + y * width].x = x;
	temp[x + y * width].y = y;
	temp[x + y * width].z = 1;
}

__global__ void setPsuedoDepth(float3* temp, float4* normal, int* labels, int width, int height){
	int x = blockIdx.x * blockDim.x + threadIdx.x;
	int y = blockIdx.y * blockDim.y + threadIdx.y;


	int l = labels[x + y * width];
	l = max(l, 0);

	float a = normal[l].x;
	float b = normal[l].y;
	float c = normal[l].z;
	float d = normal[l].w;
	float3* ref = &temp[x + y * width];

	ref->z = abs(d / (a * ref->x + b * ref->y + c));
	ref->x *= ref->z;
	ref->y *= ref->z;
}

float3* Projection_GPU::getProjectedMap(){
	initTemp<<<dim3(width / 32, height / 24), dim3(32, 24)>>>
		(temp, width, height);

	dim->applyIntrinsicInv(temp, Intrinsic_Inv, width * height);

	setPsuedoDepth<<<dim3(width / 32, height / 24), dim3(32, 24)>>>
		(temp, normal, labels, width, height);

	
	cudaMemcpy(output_CPU, temp, width * height * sizeof(float3), cudaMemcpyDeviceToHost);
	return output_CPU;
}